#include "total_inventory.h"


//-------------------------------------------------------------------------------------------------------------------
// 函数简介     高低字节融合整型  TODO
// 参数说明     high_byte      输入参数，高字节
// 参数说明     low_byte       输入参数，低字节
// 返回参数     int16_t        16位整形，不能是32位，否则数据不正确
// 使用示例
// 备注信息     两个无符号的八位整型转化为一个有符号的16位整型
//-------------------------------------------------------------------------------------------------------------------

int16_t P1010B_Merge_uint8(uint8_t high_byte,uint8_t low_byte)
{
    return ((int16_t)high_byte << 8) | low_byte;
}


//-------------------------------------------------------------------------------------------------------------------
// 函数简介     从 16 位有符号整数中提取高字节和低字节，用于提取所需电流值的高低字节   控制电机的电流环 标识符为 0x32或者0x33  TODO
// 参数说明     data            输入的浮点数，里面处理让他变为小数点为两位的浮点数，即乘以100，然后再强行转化，保证了精度
// 参数说明     *high_byte      输出参数，指向存储高字节的变量
// 参数说明     *low_byte       输出参数，指向存储低字节的变量
// 参数说明     limit_electric  电流限制，可以手动给一个电流限制，用来参数时不会出现特别大的电流
// 返回参数     void
// 使用示例
// 备注信息     将一个 16 位有符号整数分解为两个无符号的 8 位整数
// 备注信息     不通用，专属于此电机的电流环，传入的电流会在里面放大100倍，与CAN通信协议兼容
// 备注信息     电机电流峰值为75A,但是额定电流默认设置为23A，超过23A会触发过载保护，可以自行修改，预计提高到26A
//-------------------------------------------------------------------------------------------------------------------
void P1010B_Split_int16(float data, uint8_t *high_byte, uint8_t *low_byte,float limit_electric)
{

    if (high_byte != NULL && low_byte != NULL) {
        // 双重保险，对于给与限制的电流套一层锁，防止电流超限，23a为一个限制 额定为16a，过载电流极限为70A,触发过载保护的边界为23a
        if (limit_electric > 23.0f) {
            limit_electric = 23.0f; // 上限
        } else if (limit_electric < -23.0f) {
            limit_electric = -23.0f; // 下限
        }
        // 手动给的电流限制
        if (data > limit_electric) {
            data = limit_electric; // 上限
        } else if (data < -limit_electric) {
            data = -limit_electric; // 下限
        }
        data = data * 100;   // 先乘以100
        // 将数据乘以 100，并保持结果为有符号 32 位整数
        int32_t scaled_data = (int32_t)data ;
        // 提取高字节和低字节（基于补码）
        *high_byte = (uint8_t)((scaled_data >> 8) & 0xFF); // 提取高字节
        *low_byte = (uint8_t)(scaled_data & 0xFF);         // 提取低字节
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     根据给定扭矩计算所需的电流值(此电机专属)  TODO
// 参数说明     torque         给定的扭矩值（单位：Nm）
// 返回参数     float          计算得到的电流值（单位：A）
// 使用示例
// 备注信息     公式：电流值 (A) = 扭矩 (Nm) * 1.414 / 转矩常数 (1.2 Nm/A)
//-------------------------------------------------------------------------------------------------------------------

float P1010B_CalculateCurrentFromTorque(float torque)
{
    const float TORQUE_CONSTANT = 1.2f; // 转矩常数 (Nm/A)
    const float CURRENT_FACTOR = 1.414f; // 电流因子 (用于峰值到 RMS 的转换)

    // 根据公式计算电流值
    float current = torque * CURRENT_FACTOR / TORQUE_CONSTANT;
    return current;
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     根据给定电流计算所需的扭矩值(此电机专属)  TODO
// 参数说明     current        给定的电流值（单位：A）
// 返回参数     float          计算得到的扭矩值（单位：Nm）保证精度，最后的分化到发送的报文的时候再截断，乘以100再强转化整形
// 使用示例
// 备注信息     公式：扭矩 (Nm) = 电流值 (A) * 转矩常数 (1.2 Nm/A) / 1.414
//-------------------------------------------------------------------------------------------------------------------

float P1010B_CalculateTorqueFromCurrent(int current)
{
    const float TORQUE_CONSTANT = 1.2f; // 转矩常数 (Nm/A)
    const float CURRENT_FACTOR = 1.414f; // 电流因子 (用于峰值到 RMS 的转换)

    // 根据公式计算扭矩值
    float torque = (float)current * TORQUE_CONSTANT / CURRENT_FACTOR;
    return torque;
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     int类型拆成四个字节，设置参数的时候需要这么操作  TODO
// 参数说明     value           输入参数，32位的int类型
// 参数说明     *data           储存地址，即拆出来的字节的位置，可以放入数组
// 返回参数     void
// 使用示例
// 备注信息     两个无符号的八位整型转化为一个有符号的16位整型  这样解析是因为发送过来的数据为大端模式
//-------------------------------------------------------------------------------------------------------------------

void P1010B_split_int_to_bytes(int32_t value, uint8_t *data) {
    // 提取每个字节并存储到指定位置（大端模式，索引4到7）
    data[7] = (value >> 24) & 0xFF;  // 最高位字节
    data[6] = (value >> 16) & 0xFF;  // 次高位字节
    data[5] = (value >> 8) & 0xFF;   // 次低位字节
    data[4] = value & 0xFF;          // 最低位字节
}

//------------------------------------------------------------------------------------------------------------------
// 函数简介     解析电机发送的数据（大端模式），将四个字节组合成32位整数
// 参数说明     *DATA           输入参数，电机发送的字节数组（至少7字节，索引0到6）
// 返回参数     int32_t         解析后的32位有符号整数
// 使用示例     uint8_t bytes[7] = {0x00, 0x00, 0x00, 0x12, 0x34, 0x56, 0x78};
//              int32_t result = parse_motor_data(bytes); // result = 0x12345678
// 备注信息     按照大端模式解析，DATA[3]为最高位字节，DATA[6]为最低位字节
//-------------------------------------------------------------------------------------------------------------------
int32_t parse_motor_data(uint8_t *DATA)
{
    int32_t data = (DATA[3] << 24) | (DATA[4] << 16) | (DATA[5] << 8) | DATA[6];
    return data;
}


#if MACRO_VALUE == 1            // 这里根据头文件里面的编译来进行，对于不需要的时候可以直接不会进行编译，减小单片机内存
//—————————————————————————————————————————————————基础CAN通信数组(P1010B电机专属)——————————————————————————————————————————————————
// 定义变量名前缀为 P1010B 的数组（精简版）
// 标识符为 0x38
uint8_t P1010B_En[8] = {0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02}; // 使能指令
uint8_t P1010B_Dis[8] = {0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01}; // 失能指令

// 下面的模式切换以及ID切换必须要在失能模式下来进行操作                    标识符为 0x36
uint8_t P1010B_Open[8] = {0x01, 0x1C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; // 开环模式
uint8_t P1010B_Curr[8] = {0x01, 0x1C, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00}; // 电流环模式
uint8_t P1010B_Speed[8] = {0x01, 0x1C, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00}; // 速度环模式
uint8_t P1010B_Pos[8] = {0x01, 0x1C, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00}; // 位置环模式

// ID切换一般为控制多个电机的时候有ID重了的情况，此时在失能的情况下来切换ID    标识符为 0x36
uint8_t P1010B_ID1to2[8] = {0x01, 0x2A, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00}; // 将 ID1 设置为 ID2
uint8_t P1010B_ID1to3[8] = {0x01, 0x2A, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00}; // 将 ID1 设置为 ID3
uint8_t P1010B_ID1to4[8] = {0x01, 0x2A, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00}; // 将 ID1 设置为 ID4
uint8_t P1010B_ID2to1[8] = {0x02, 0x2A, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00}; // 将 ID2 设置为 ID1

// 数据查询代码(1-14)
//  1 中心轴速度 * 10(中心轴转速，分辨率0.1；即反馈值除以10得到正确数值，单位:RPM，需要转化为角速度)
//  2 母线电流 * 100
// 3 Iq * 100(返回变量电流：IQ实际反馈电流，分辨率0.01(即：反馈数值 / 100为实际IQ电流)；)
// 4 转子位置 (0-32768) 通过比例去换算弧度制或者角度 这里是增量式，会进行清零，如果需要绝对这编码器，则需要电流输入的反馈来得到
// 5 故障信息                   6 警告信息
// 7 MOS 温度                  8 电机绕组温度
// 9 当前模式                   10 当前系统电压 * 10
// 11 当前转的圈数累计 * 100      12 当前系统状态
// 13 绝对位置 (0-32768)        14 相电流最大值 * 100a      相电流代表负载出现变化可以用来补偿或者动态算法

// 设置数据查询模式 标识符为 0x34 主动上报模式 第一个数据为01 代表主动查询模式，第二个数据为 01 代表主动上报模式，
// 假设设置为主动上报模式，接收的标识符为 0xC1    假设设置为主动查询莫模式，接收的标识符为   0x71    ID来区分不同的数据赋值给不同的数组，定时中断直接处理数组
// 这里的反馈标识符都针对电机的ID为1，对于主动查询，目前还需要验证不同ID是否不一样
// 主动上报模式，数组第四个字节到第7个字节为需要定时上报的数据查询代码，第三个字节为定时上报的周期(ms单位)
// 主动查询模式需要设置数据接收为查询模式，然后继续发送数组来获取你需要的数据，标识符为 0x36，前面四个字节为你需要获取的数据查询代码


//标识符为0x34 自动上报模式  反馈数据标识符为 0xC1 第一个参数为电机ID
// 查询数据按顺序为： 中心轴速度 母线电流 IQ电流 转子位置     感觉没啥必要
uint8_t P1010B_autoReported[8] = {0x01,0x01,0x05,0x01,0x02,0x03,0x04,0}; //  查询模式设置

//标识符为 0x35 主动查询模式 ，反馈数据标识符为 0x71(0x70 + 电机ID)
// 查询数据为： 当前系统电压*10  当前转的圈数累计 * 100  绝对位置 (0-32768)    相电流最大值 * 100a
uint8_t P1010B_TxData_findData[8] = {0x0A,0x0B,0x0D,0x0E,0,0,0,0};   // 设置主动查询模式后就可以使用此代码来主动查询数据


// 标识符为0x32 电流环指令(处于电流模式下)  设置电流为0a,特殊情况下来减速
// 电流环发送数据分为四组，在标识符为 0x32的情况下，ID依次为1-4的电机驱动数据，所以每一组数据对应一个ID
//+++================= 极其重要！！   电流环反馈当前的速度，输入一次反馈一次，由于电机不停的输入，所以可以一直查看反馈的值
//+++================= 极其重要！！   第一组([0]和[1])数据为速度，第二组数据为IQ，第三组为绝对值位置，第四组为电压位置，
//+++================= 极其重要！！上电就需要检测当前角度作为初始角度，所以上电后需要发送一次，反馈标识符为 （0x50+ID）
uint8_t P1010B_electric_zero[8] = {0,0,0,0,0,0,0,0}; //  电流 0a 用来关停电机  标识符0x32
uint8_t P1010B_electric[8] = {0x03,0xE8,0,0,0,0,0,0}; //  电流 10a 当然里面可以替换为所需要输入的电流数据

// 标识符为0x36  [1]为电机ID [2]-[6]为你需要设置的参数，这里用于额定电流所以序号为0x3D(61)  电流设置的额定电流为你想要设置的电流乘以100
// 设置的参数遵循  (DATA[5]<<24) | (DATA[4]<<16) | (DATA[3]<<8) | DATA[2] )
// 反馈报文的标识符位0x80 + 电机ID  [1]为电机ID [2]-[6]为你需要设置的参数
uint8_t P1010B_messageSet[8] = {0x03,0x3D,0,0,0,0,0,0}; //  电流 10a 当然里面可以替换为所需要输入的电流数据

//—————————————————————————————————————————————————基础CAN通信数组(P1010B电机专属)——————————————————————————————————————————————————

#endif

//—————————————————————————————————————————————————电机前馈系统设计——————————————————————————————————————————————————

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     负载扭矩增益结构体初始化   TODO
// 参数说明     DOBState *state  引入结构体参数
// 参数说明     kp_iq            放大负载扭矩增益的系数
// 参数说明     zero_iq          空载IQ电流的参数  此电机为 1.7
// 返回参数     void
// 使用示例
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
// 初始化DOB状态参数
void P1010B_dob_init(DOBState *state, float kp_iq,float zero_iq) {
    state->KP_IQ = kp_iq;
    state->Zero_IQ = zero_iq;
    state->d_hat  = 0.0f;
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     DOB计算函数(里面的参数换算仅适用于此电机，其他电机需要改变扭矩和电流的换算)  TODO
// 参数说明     DOBState *state  引入结构体参数
// 参数说明     high_byte      输入参数，高字节  IQ电流  Can_rxData[2]   这里对应电流反馈的数据 返回的标识符 0x50 + ID
// 参数说明     low_byte       输入参数，低字节  IQ电流  Can_rxData[3]
// 返回参数     float          负载扭矩增益估计(叠加到前馈上面,单位NM)
// 使用示例
// 备注信息     使用的前提是前面结构体参数初始化成功
//-------------------------------------------------------------------------------------------------------------------
float P1010B_dob_update(DOBState *state,uint8_t high_byte,uint8_t low_byte)
{
    const float KT = 1.2f;          // 扭矩常数
    const float SQRT_2 = 1.414f;   // 扭矩和电流换算公式的参数

    float IQ = (float)(P1010B_Merge_uint8(high_byte,low_byte) * 0.001);

    float Tau_actual = (KT * IQ) / SQRT_2;                // 负载扭矩
    float Zero_actual =  (float)((KT * state->Zero_IQ) / SQRT_2);    // 零负载的空载扭矩

    // 计算扰动
    state->d_hat = (Tau_actual - Zero_actual) * state -> KP_IQ;

    // 非负增益，确保增益始终为正
    if(state->d_hat < 0.0f)
    {
        state->d_hat = 0.0f;
    }

    // 计算当前实际扭矩
    return state->d_hat;
}


//-------------------------------------------------------------------------------------------------------------------
// 函数简介     查表法初始化(用于计算tanh函数)  TODO
// 参数说明     TanhTable *table       引入结构体参数
// 参数说明     min                    表的范围里的最小值
// 返回参数     max                    表的范围里的最大值
// 使用示例     P1010B_init_tanh_table(&tanhs,-2,2,1000);
// 备注信息     这里范围越小对于tanh来说越平滑，而范围越小越容易达到饱和
// 备注信息     查表法通过将输入范围-3到3进行tanh计算，离散到1000个数据里面，存储在数组里，这样减少实时计算开销
// 备注信息     输入范围可以更变，越大越容易饱和，越低越能保留更多细节，但是过低则丧失了非线性特性
//-------------------------------------------------------------------------------------------------------------------
// 初始化查表
void P1010B_init_tanh_table(P1010B_TanhTable *table, float min, float max, int size)
{
    if (size <= 0 || size > 1000) {
        return;
    }
    // 设置结构体字段
    table->x_min = min;
    table->x_max = max;
    table->table_size = size;

    // 填充查表
    float step = (max - min) / (size - 1);
    for (int i = 0; i < size; i++) {
        float x = min + i * step;
        table->tanh_table[i] = tanh(x);  // 使用标准库函数预先计算
    }

}


//-------------------------------------------------------------------------------------------------------------------
// 函数简介     查表法计算tanh函数用于非线性缩放  TODO
// 参数说明     P1010B_TanhTable *table引入结构体参数
// 参数说明     x                      传入的数值，但是还需要引入敏感系数，这里暂未作为传递参数
// 参数说明     error_max              误差的最大值，对于编码器数值就是32768  顺时针为正，逆时针为逆
// 参数说明     k_a                    比例增益，调整敏感因子，敏感因子增大，则越容易达到饱和，反之则更难
// 返回参数     tanh计算后的值           至于值未多少取决于你的归一化的范围
// 使用示例     P1010B_init_tanh_table(&tanhs,-2,2,1000);    // 前馈初始化 缩放范围
// 备注信息     这里范围越小对于tanh来说越平滑，而范围越大越容易达到饱和，越容易达到饱和则在小误差内控制更加精细但是在大误差里面就容易达到饱和
// 备注信息     函数内部调用，无需用户关心
//-------------------------------------------------------------------------------------------------------------------
// 使用查表法计算 tanh
float P1010B_tanh_lookup(const P1010B_TanhTable *table, float x,float error_max,float k_a) {
    if (table->table_size == 0) {
        printf("Error: Tanh table is not initialized.\n");
        return 0.0;
    }

    float a = table->x_max / error_max;

    x = x * a;  // 进行敏感因子缩放，即将误差范围归一到 -5 到 5 的范围
    x = x * k_a;  // 比例增益调整敏感因子


    // 超出范围处理,直接等于边界值介于饱和
    if (x <= table->x_min) x = table->x_min;
    if (x >= table->x_max) x = table->x_max;
//    printf("%f\r\n",x);
    // 计算步长和索引
    float step = (table->x_max - table->x_min) / (table->table_size - 1);
    int index = (int)((x - table->x_min) / step);

//    printf("%d\r\n",index);

    // 线性插值
    float x_low = table->x_min + index * step;
    float x_high = table->x_min + (index + 1) * step;
    float y_low = table->tanh_table[index];
    float y_high = table->tanh_table[index + 1];

    return y_low + (y_high - y_low) * (x - x_low) / (x_high - x_low);
}



//-------------------------------------------------------------------------------------------------------------------
// 函数简介     非线性缩放前馈扭矩增益的最终函数，里面会调用前面的定义的函数  TODO
// 参数说明     TanhTable *table       引入结构体参数
// 参数说明     x                      传入的数值，但是还需要引入敏感系数，这里暂未作为传递参数
// 参数说明     error_max              误差的最大值，对于编码器数值就是32767
// 参数说明     k_a                    比例增益，调整敏感因子，敏感因子增大，则越容易达到饱和，反之则更难
// 参数说明     k_out                  比例增益，用于缩放基础扭矩，然后再进行非线性映射,基础前馈扭矩比较小，所以通过比例扩大
// 参数说明     d_hat                  电机前馈扭矩的补偿，用于外部模块直接输入
// 返回参数     T_scaled               总的前馈扭矩，浮点数
// 使用示例     compute_scaled_torque(&tanhs,(Target_angle -Actual_angle),32768,0.6,10.0,0.0);    // 目前数据比较合理，但是依赖前面初始化的范围为[-3,3]
// 备注信息     这里范围越小对于tanh来说越平滑，而范围越大越容易达到饱和，越容易达到饱和则在小误差内控制更加精细但是在大误差里面就容易达到饱和
//            非线性的输入范围目前为-3到3的范围，能够保留更多的细节，防止过早饱和，如果想要很早饱和，可以扩大到5到-5
//            所以需要将误差范围缩放到-3到3的范围，因为tanh只能输入这么大的范围达到有效非线性区域
//            这种非线性缩放的方法，在小误差的时候变化比较缓慢，大误差的时候变化比较抖，与指数平滑相反
//-------------------------------------------------------------------------------------------------------------------
float compute_scaled_torque(const P1010B_TanhTable *table,float error,float error_max,float k_a,float k_out,float d_hat)
{
//——————————————————————  固定参数不允许修改
    const float Ts = 0.01f;         // 计算周期
    const float Target_w = 10.0f;   // 前馈的期望角速度
    float α = Target_w / Ts;        // 前馈的期望角加速度
    const float J = 0.000535;       // 转动惯量
    float T_ff = J * α;             // 计算的动力学基础前馈，在这个基础上进行增益和调整缩放大小
//——————————————————————  固定参数不允许修改

    float T_scaled = T_ff * k_out * P1010B_tanh_lookup(table,error,error_max,k_a) + d_hat;   // 进行非线性比例缩放后的力矩输出
//    printf("%f\r\n",P1010B_tanh_lookup(table,error,error_max,k_a));
    float i = P1010B_CalculateCurrentFromTorque(T_scaled);

    if (i <= -20.0f) i = -20.0f;
    if (i >= 20.0f)  i = 20.0f;       // 限制电流范围，最大前馈为20A


    return i;

}



//—————————————————————————————————————————————————电机前馈系统设计——————————————————————————————————————————————————



//—————————————————————————————————————————————————电机前馈系统配合的反馈系统设计———————————————————————————————————————————

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     基于前馈的反馈系统初始化   TODO
// 参数说明     P1010B_PD *pd    引入结构体参数
// 参数说明     kp               比例系数
// 参数说明     kd               比例系数
// 返回参数     void
// 使用示例     P1010B_initPDController(&PD_F,0.002,0.18);  // 初始化反馈机制
// 备注信息     使用示例的参数适用于此电机
//-------------------------------------------------------------------------------------------------------------------
// 初始化PD控制器
void P1010B_initPDController(P1010B_PD *pd, float kp, float kd) {
    pd->kp = kp;
    pd->kd = kd;
    pd->set_angle = 0.0;
    pd->set_speed = 0.0;     // 目标速度为0
}


//-------------------------------------------------------------------------------------------------------------------
// 函数简介     反馈系统计算函数  TODO
// 参数说明     P1010B_PD *pd      引入结构体参数
// 参数说明     current_value:     当前角度
// 参数说明     current_speed:     当前角速度 (rad/s)
// 返回参数     float              反馈计算的值
// 使用示例
// 备注信息     使用的前提是前面结构体参数初始化成功，类似PID中的KD效果
//-------------------------------------------------------------------------------------------------------------------

// 计算PD控制输出
float P1010B_computePDControl(P1010B_PD *pd, float current_value,float current_speed) {
    // 计算当前误差
    float error = pd->set_angle - current_value;

    // 计算控制输出：u(t) = Kp * e(t) + Kd * de(t)/dt
    float output = pd->kp * error + pd->kd * (pd->set_speed - current_speed);

    return output;
}


//—————————————————————————————————————————————————电机前馈系统配合的反馈系统设计———————————————————————————————————————————


//—————————————————————————————————————————————————电机前馈和反馈的融合设计———————————————————————————————————————————

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     前馈配合反馈的结合计算  TODO
// 参数说明     P1010B_PD *pd                引入反馈结构体参数
// 参数说明     P1010B_TanhTable *table      引入前馈结构体参数
// 参数说明     target_angle:                目标旋转角度，绝对值
// 参数说明     total_angle:                 电机已经旋转过了的绝对值角度
// 参数说明     D_hat                        外部DOB负载扰动增益输入
// 参数说明     *high_byte                   输出参数，指向存储高字节的变量
// 参数说明     *low_byte                    输出参数，指向存储低字节的变量
// 返回参数     void                         无反馈
// 使用示例
// 备注信息     这个函数的使用的前提是前面结构体正确初始化，且引入外部DOB负载扰动增益输入
//-------------------------------------------------------------------------------------------------------------------
void Feedforward_PDC(P1010B_PD *pd,P1010B_TanhTable *table,int target_angle,int total_angle,float Actual_speed,float D_hat,uint8_t *high_byte, uint8_t *low_byte)
{
    float OUTPUT = compute_scaled_torque(table,(target_angle -total_angle),32768,0.6,35.0,D_hat);     // raw_delta 为绝对角度
    OUTPUT += P1010B_computePDControl(pd,total_angle,Actual_speed);
//———————————————————————————————————————————————可以尝试用速度环来模拟负载情况，即扭矩

    if (OUTPUT <= -23.0f) OUTPUT = -23.0f;
    if (OUTPUT >= 23.0f)  OUTPUT = 23.0f;       // 限制电流范围，最大前馈为23A

    P1010B_Split_int16(OUTPUT,high_byte,low_byte,23.0); // 输出电流限制为23A
}

//—————————————————————————————————————————————————电机前馈和反馈的融合设计———————————————————————————————————————————







